#!/usr/bin/env python
# coding=UTF-8

import rospy, sys, select, termios, tty
import time
import handsfree_arm_msgs.msg as handsfree_arm_msg


def talker(position):
    rospy.init_node('set_arm_angles', anonymous=True)
    pub = rospy.Publisher('/handsfree/arm/set_arm_joints_pos', handsfree_arm_msg.arm_joints_position, queue_size=10)
    rate = rospy.Rate(10)
    pub.publish(position)
    rate.sleep()

def getch(timeout):
    # 返回单字符键盘输入
    fd = sys.stdin.fileno()
    old = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        (result_stdin, w, x) = select.select([sys.stdin], [], [], timeout)
        if len(result_stdin):
            return result_stdin[0].read(1)
        else:
            return False

        # return sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old)

if __name__ == '__main__':
    key = ''
    # 键盘监听
    while not rospy.is_shutdown():
        key = getch(0.3)

        # 初始查看
        if key == '1':
            talker([0.0, -0.11170107126235962, 0.11711159348487854, 0.0, -0.7836528420448303, 0.0, 1.0])
            time.sleep(0.1)
        # 摇摆查看
        if key == '2':
            talker([0.0, -0.11170107126235962, 0.11711159348487854, 0.0, -0.7836528420448303, 0.0, 1.0])
            time.sleep(0.1)
            talker([0.0, -0.11170107126235962, 0.11711159348487854, 0.0, -0.7836528420448303, 0.0, 1.0])
            time.sleep(0.1)
        # 前往目标点
        if key == '3':
            talker([0.0, 0.8674286603927612, 0.455705463886261, 0.0, -2.8413960933685303, 0.0, 1.0])
            time.sleep(0.1)
        # 目标点位置合上夹爪
        if key == '4':
            talker([0.0, 0.8674286603927612, 0.455705463886261, 0.0, -2.8413960933685303, 0.0, 0.3])
            time.sleep(0.1)
        # 抓取物体后移动位姿
        if key == '5':
            talker([0.0, -0.6667158007621765, 0.32480576634407043, -0.0, -1.2217304706573486, 0.0, 0.3])
            time.sleep(0.1)